#include "motor_ctrl_task.h"
#define PI 3.14159265
motor_t motor[4];
rm_motor_groupTypeDef motor_chassis;
pid_controller motor_chassis_pid[4] = {};

void StartMotorCtrlTask(void *argument)
{
    UNUSED(argument);
    vTaskDelay(2000);
    for (int i = 0; i < 4; i++)
    {
        dm_motor_init(motor + i, i + 1, 0x141 + i);
        dm_motor_enable(&hcan2, motor + i);
        vTaskDelay(10);
    }
    rm_motor_init(&motor_chassis, 0x200, &hcan2);
    rm_motor_cmd_max_config(&motor_chassis, 16384, 16384, 16384, 16384);

    for (int i = 0; i < 4; i++)
    {
        pid_controller_create(motor_chassis_pid + i, 5, 0, 0, 100, 16384);
    }
    

    vTaskDelay(10);
    float leg_angle_front = PI / 3;
    float leg_angle_rear = PI / 3;
    float speed_x = 0;
    float speed_z = 0;
    float chassis_speed_target[4] = {};

    while (1)
    {
        speed_x = (dbus_ctrl_data.channel.CH[1] - 0x400) * 5;
        speed_z = (dbus_ctrl_data.channel.CH[0] - 0x400) * 5;
        leg_angle_front += ((float)(dbus_ctrl_data.channel.CH[3] - 0x400) * 0.0001);
        leg_angle_rear += ((float)(dbus_ctrl_data.channel.CH[2] - 0x400) * 0.0001);
        while (leg_angle_front > 2 * PI)
        {
            leg_angle_front -= (2 * PI);
        }
        while (leg_angle_front < -2 * PI)
        {
            leg_angle_front += (2 * PI);
        }
        
        while (leg_angle_rear > 2 * PI)
        {
            leg_angle_rear -= (2 * PI);
        }
        while (leg_angle_rear < -2 * PI)
        {
            leg_angle_rear += (2 * PI);
        }
        
        
        
        if (dbus_ctrl_data.channel.s1 == 0x01)
        {
            mit_ctrl(&hcan2, motor + 0, 1, leg_angle_front, 0, 40, 4, 0);
            mit_ctrl(&hcan2, motor + 1, 2, - leg_angle_rear, 0, 40, 4, 0);
            mit_ctrl(&hcan2, motor + 2, 3, - leg_angle_front, 0, 40, 4, 0);
            mit_ctrl(&hcan2, motor + 2, 4, leg_angle_rear, 0, 40, 4, 0);
            
            chassis_speed_target[0] = -speed_x + speed_z;
            chassis_speed_target[1] = -speed_x + speed_z;
            chassis_speed_target[2] = speed_x + speed_z;
            chassis_speed_target[3] = speed_x + speed_z;
            
            for (int i = 0; i < 4; i++)
            {
                pid_error_input(motor_chassis_pid + i, (float)chassis_speed_target[i] - (float)motor_chassis.motor_data[i].speed);
            }
            

            rm_motor_cmd_refresh(&motor_chassis, motor_chassis_pid[0].output,
                                                 motor_chassis_pid[1].output, 
                                                 motor_chassis_pid[2].output, 
                                                 motor_chassis_pid[3].output);
            rm_motor_send_msg(&motor_chassis);
            /*vTaskDelay(1000);
            
            mit_ctrl(&hcan2, motor + 1, 2, 0, 0, 40, 4, 0);
            
            vTaskDelay(1000);
            mit_ctrl(&hcan2, motor + 2, 3,  PI / 3, 0, 40, 4, 0);
            vTaskDelay(500);
            mit_ctrl(&hcan2, motor + 2, 3,  PI / 6, 0, 40, 4, 0);
            vTaskDelay(500);
            mit_ctrl(&hcan2, motor + 2, 3,  PI / 3, 0, 40, 4, 0);
            vTaskDelay(500);*/

        }
        else
        {
            mit_ctrl(&hcan2, motor + 0, 1, PI / 3, 0, 40, 4, 0);
            mit_ctrl(&hcan2, motor + 1, 2, - PI / 3, 0, 40, 4, 0);
            mit_ctrl(&hcan2, motor + 2, 3, - PI / 3, 0, 40, 4, 0);
            mit_ctrl(&hcan2, motor + 2, 4, PI / 3, 0, 40, 4, 0);
            float force[4] = {};
            float torque[4] = {};
        }
        
        vTaskDelay(10);
    }
}